/******************************************************************************/
/* File: utilClass.cpp                                                        */
/******************************************************************************/

#include "utilClass.h"

/*
在这里，我们定义了三种延时器，分别是：
1 基于nanosleep()的延时器
2 基于gettimeofday()函数循环的延时器
3 基于Kalman Filter(卡尔曼滤波)原理的延时器
默认使用nanosleep的延时器
*/
void UtilClass::DelayLoop(unsigned long usecs) {
    
    DelayNanosleep(usecs);
    //DelayBusyloop(usecs);
    //DelayKalman(usecs);
}

void UtilClass::DelayNanosleep (unsigned long usec) {
    
    struct timespec requested = {0, 0}, remaining;
    
    requested.tv_nsec += usec * 1000L;
    
    while (requested.tv_nsec >= BILLION) {
	    requested.tv_sec++;
	    requested.tv_nsec -= BILLION;
    }

    // Note, signals will cause the nanosleep
    // to return early.  That's fine.
    nanosleep(&requested, &remaining);
}

void UtilClass::DelayBusyloop (unsigned long usec) {
    struct timeval t1, t2;
    
    gettimeofday(&t1, NULL);
    TimevalAddLong(&t1, usec);
     
    while (1) {	
	    gettimeofday(&t2, NULL);
	    if (TimevalGreaterThan(t2, t1)) 
	        break;
    }
}

// Kalman versions attempt to support delay request
// accuracy over a minimum guaranteed delay by
// prediciting the delay error. This is
// the basic recursive algorithm. 
void UtilClass::KalmanUpdate (KalmanState *state, double measurement) {
    //prediction update
    state->p = state->p + state->q;
    //measurement update
    state->k = state->p / (state->p + state->r);
    state->x = state->x + (state->k * (measurement - state->x));
    state->p = (1 - state->k) * state->p; 
}

// Request units is microseconds
// Adjust units is microseconds
void UtilClass::DelayKalman (unsigned long usec) {
    struct timeval t1, t2, finishtime;
    long usec_adjusted; 
    double err;
    static KalmanState kalmanerr={
	    0.00001, //q process noise covariance
	    0.1, //r measurement noise covariance
	    0.0, //x value, error predictio (units nanoseconds)
	    1, //p estimation error covariance
	    0.25 //k kalman gain
    };
    
    // Get the current clock
    gettimeofday(&t1, NULL);
    // Perform the kalman adjust per the predicted delay error
    if (kalmanerr.x > 0) {
	    usec_adjusted = usec - (long)(kalmanerr.x);
	    if (usec_adjusted < 0) 
	        usec_adjusted = 0;
    }
    else 
	    usec_adjusted = usec + (long)(kalmanerr.x);
    
    // Set the finishtime
    finishtime = t1;
    TimevalAddLong(&finishtime, usec_adjusted);
    
    // Don't call nanosleep for values less than 10 microseconds
    // as the syscall is too expensive.  Let the busy loop
    // provide the delay for times under that.
    if (usec_adjusted > 10) {
	    struct timespec requested={0,0}, remaining;
	    TimespecAddLong(&requested, (usec_adjusted * 1000));
	    nanosleep(&requested, &remaining);
    }
    
    while (1) {
	    gettimeofday(&t2, NULL);
	    if (TimevalGreaterThan(t2, finishtime)) 
	        break;
    }
    
    // Compute the delay error in units of microseconds
    // and cast to type double
    err = (double)(TimevalDiff(t2, t1)  - usec);
    // printf("req: %ld adj: %ld err: %.5f (us)\n", usec, usec_adjusted, kalmanerr.x);  
    KalmanUpdate(&kalmanerr, err);
}

// Sadly, these systems must use the not so efficient gettimeofday()
// and working units are microseconds, struct is timeval
void UtilClass::TimevalAddLong (struct timeval *tv0, unsigned long value) {
    tv0->tv_usec += value;
    while (tv0->tv_usec >= MILLION) {
	    tv0->tv_sec++;
	    tv0->tv_usec -= MILLION;
    }
}

// Delay calls for systems with clock_gettime
// Working units are nanoseconds and structures are timespec
void UtilClass::TimespecAddLong (struct timespec *tv0, unsigned long value) {
    tv0->tv_nsec += value;
    while (tv0->tv_nsec >= BILLION) {
	    tv0->tv_sec++;
	    tv0->tv_nsec -= BILLION;
    }
}

// if tv1 > tv2, then return 1
int UtilClass::TimevalGreaterThan(struct timeval tv1, struct timeval tv0) {
    if (tv1.tv_sec > tv0.tv_sec || ((tv0.tv_sec == tv1.tv_sec) && (tv1.tv_usec > tv0.tv_usec)))
	    return 1;
	
    return 0;
}

// tv1 assumed greater than tv0, return (tv1 - tv2)
double UtilClass::TimevalDiff (struct timeval tv1, struct timeval tv0) {
    double result;
    if (tv1.tv_usec < tv0.tv_usec) {
	    tv1.tv_usec += MILLION;
	    tv1.tv_sec--;
    }
    
    result = (double) (((tv1.tv_sec - tv0.tv_sec) * MILLION) + (tv1.tv_usec - tv0.tv_usec));
    return result;
}

/* -------------------------------------------------------------------
 * pattern
 *
 * Initialize the buffer with a pattern of (index mod 10).
 * ------------------------------------------------------------------- */
void UtilClass::PatternBuffer(char *outBuf, int inBytes ) {
    if(outBuf == NULL)
        return;

    while (inBytes-- > 0) {
        outBuf[inBytes] = (inBytes % 10) + '0';
    }
}

long UtilClass::ByteAtoI( const char *inString ) {
    
    const long Kilo_to_Unit = 1024;
    const long Mega_to_Unit = 1024 * 1024;
    const long Giga_to_Unit = 1024 * 1024 * 1024;

    const long kilo_to_Unit = 1000;
    const long mega_to_Unit = 1000 * 1000;
    const long giga_to_Unit = 1000 * 1000 * 1000;
    
    double theNum;
    char suffix = '\0';
    
    if(inString == NULL)
        return 0;

    /* scan the number and any suffices */
    sscanf( inString, "%lf%c", &theNum, &suffix );

    /* convert according to [Gg Mm Kk] */
    switch ( suffix ) {
        case 'G':  
            theNum *= Giga_to_Unit;  
            break;
        case 'M':  
            theNum *= Mega_to_Unit;  
            break;
        case 'K':  
            theNum *= Kilo_to_Unit;  
            break;
        case 'g':  
            theNum *= giga_to_Unit;  
            break;
        case 'm':  
            theNum *= mega_to_Unit;  
            break;
        case 'k':  
            theNum *= kilo_to_Unit;  
            break;
        default: 
            break;
    }
    
    return (long) theNum;
}

void UtilClass::DebugPrint(const char *debugInfo) {
    std::cout << debugInfo << std::endl;
}

/* -------------------------------------------------------------------
 * RegisterSignalHandler函数
 *
 * installs a signal handler, and returns the old handler.
 * This emulates the semi-standard signal() function in a
 * standard way using the Posix sigaction function.
 * ------------------------------------------------------------------- */
SigfuncPtr UtilClass::RegisterSignalHandler( int inSigno, SigfuncPtr inFunc ) {
    struct sigaction theNewAction, theOldAction;

    if(inFunc == NULL)
        return NULL;

    theNewAction.sa_handler = inFunc;
    sigemptyset( &theNewAction.sa_mask );
    theNewAction.sa_flags = 0;

    if ( sigaction( inSigno, &theNewAction, &theOldAction ) < 0 ) {
        return SIG_ERR;
    } else {
        return theOldAction.sa_handler;
    }
}

/* -------------------------------------------------------------------
 * SigExit函数
 *
 * Quietly exits. This protects some against being called multiple
 * times. (TODO: should use a mutex to ensure (num++ == 0) is atomic.)
 * ------------------------------------------------------------------- */
void UtilClass::SigExit( int inSigno ) {
    static int num = 0;
    if ( num++ == 0 ) {
        fflush( 0 );
        exit( 0 );
    }
}

/******************************************************************************/
/* End File: utilClass.cpp                                                    */
/******************************************************************************/